/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**Fleet Formation Controller
 *
 * Controller which controls an entire fleet. One of the fleet machine
 * is the "master" which controls other machine's coordinateDrive to move in a wanted formation.
 *
 * \file
 * \brief
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#ifndef _FLEET_FORMATION_CONTROLLER
#define _FLEET_FORMATION_CONTROLLER

#include "CoordinateDriveClient.hpp"
#include "PositionClient.hpp"
#include "SpeedCtrlServer.hpp"
#include "SpeedCtrlClient.hpp"
#include <vector>
#include <map>
#include "owndebug.h"
#include "ownutils.h"
#include "binbag.h"

enum EFleetDrivingStyle{
  
  KFleetDriveFormationStaysStatic = 0,
  KFleetDriveFormationRelativeToMasterHeading = 1

};

class CFleetFormationController : private gim::CSync,
                                  private gim::CThread,
                                  public MaCI::Position::CPositionClientCallback,
                                  public MaCI::SpeedCtrl::CSpeedCtrlClientCallback
                                  
{
  


public:
  
  CFleetFormationController();
  virtual ~CFleetFormationController();
  
  bool SetMasterMachine(MaCI::Position::CPositionClient *iMasterPositionClient,
                        MaCI::SpeedCtrl::CSpeedCtrlClient *aMasterSpeedCtrlClient,
                        std::string aMaCIGroup ="");
  
  bool GetMasterMachine(MaCI::Position::CPositionClient *aMasterPositionClient);

  bool RemoveMasterMachine();  
  /**
   * Adds a slave machine with a relative position 
   *
   */
  bool AddSlaveMachine(MaCI::Position::CPositionClient *iSlavePositionClient,
                       MaCI::SpeedCtrl::CSpeedCtrlClient *iSlaveSpeedCtrlClient,
                       MaCI::Position::TPose2D aPose);

    
  bool RemoveSlaveMachine(MaCI::Position::CPositionClient *aSlavePositionClient);

  bool RemoveAllSlaves();

  void StartToFollow();

  void StopToFollow();
  
  void GoToFormation();

  void SetFleetDrivingStyle(EFleetDrivingStyle aStyle);


 protected:
  //Position client callback
  void OnPositionDataEvent(const MaCI::Position::CPositionData &aData, 
                           const unsigned int aDataSequence,
                           MaCI::Position::CPositionClient &aPositionClient,
                           void * aUserPtr = NULL);
  //SpeedCtrl client callback
  void OnSpeedCtrlDataEvent(const MaCI::SpeedCtrl::CSpeedCtrlData &aData, 
                            const unsigned int aDataSequence,
                            MaCI::SpeedCtrl::CSpeedCtrlClient &aSpeedCtrlClient,
                            void * aUserPtr = NULL);

  int ThreadFunction(const int aThreadNumber);
  int SpeedCtrlServerThread();

private:

  MaCI::Position::CPositionClient *iMasterPositionClient;
  MaCI::SpeedCtrl::CSpeedCtrlServer *iMasterSpeedCtrlServer;
  MaCI::SpeedCtrl::CSpeedCtrlClient *iMasterSpeedCtrlClient;
  MaCI::MaCICtrl::CMaCICtrlServer *iMCS;
  MaCI::Position::TPose2D iMasterPosition;
  MaCI::SpeedCtrl::TSpeed iMasterSpeed;
  gimi::GIMI *iGimi;
  
  std::vector<MaCI::Position::CPositionClient *> iSlavePositionClientArray;
  std::map<MaCI::Position::CPositionClient *,MaCI::SpeedCtrl::CSpeedCtrlClient *>iSlaveSpeedCtrlMap;
  std::map<MaCI::Position::CPositionClient*,MaCI::Position::TPose2D> iSlaveRelativePositionsMap;
 
  
  bool iFollow;
  bool iGoToFormation;

  //  bool iIsInFormation;

  MaCI::SpeedCtrl::TSpeed iSlavesMaxSpeed;
  float iTolerance;
  float iAngleTolerance;
  float iAngularSpeedRatio;
  float iDrivingSpeed;

  EFleetDrivingStyle iDrivingStyle;


  bool iSpeedCtrlServerThreadRunning;
  bool iSpeedCtrlServerThreadStopped;
  void DoSlaveControlling(MaCI::Position::CPositionClient *aPositionClient,
                          const MaCI::Position::TPose2D *aSlaveCurrentPose);
  float NormalizeAngle(float aAngle);
  void GoToFormation(MaCI::Position::CPositionClient *aPositionClient,
                     const MaCI::Position::TPose2D *aSlaveCurrentPose);
  //bool IsInFormation();
  void DoFormationStaticDriving(MaCI::Position::CPositionClient *aPositionClient,
                                const MaCI::Position::TPose2D *aSlaveCurrentPose);
  void DoFormationRelativeToMasterHeading(MaCI::Position::CPositionClient *aPositionClient,
                                          const MaCI::Position::TPose2D *aSlaveCurrentPose);
 // Private copy const.
  CFleetFormationController(const CFleetFormationController &) 
    :  CSync(1,1),
       MaCI::Position::CPositionClientCallback(),
       MaCI::SpeedCtrl::CSpeedCtrlClientCallback(),
       iMasterPositionClient(NULL),
       iMasterSpeedCtrlServer(NULL),
       iMasterSpeedCtrlClient(NULL),
       iMCS(NULL),
       iMasterPosition(),
       iMasterSpeed(),
       iGimi(NULL),
       iSlavePositionClientArray(),
       iSlaveSpeedCtrlMap(),
       iSlaveRelativePositionsMap(),
       iFollow(false),
       iGoToFormation(false),
       iSlavesMaxSpeed(),
       iTolerance(0.0),
       iAngleTolerance(0.0),
       iAngularSpeedRatio(0.0),
       iDrivingSpeed(0.0),
       iDrivingStyle(KFleetDriveFormationRelativeToMasterHeading),
       iSpeedCtrlServerThreadRunning(false),
       iSpeedCtrlServerThreadStopped(false){}

    
 
  CFleetFormationController &operator=(const CFleetFormationController &) { return *this; }

};


#endif
